package touchphysics.physics.collision;

import touchphysics.math.Vector2f;

/**
 *  
 * @author Valerij Wittenbeck
 *
 */
public class PenetrationSweep {
	
	/**  */
	private Vector2f normal;
	/**  */
	private Vector2f sweepDir;
	/**  */
	private float startDist;
	/**  */
	private float endDist;
	
 	/**
 	 * 
 	 * @param normal 
 	 * @param sweepDir 
 	 * @param intersectionStart 
 	 * @param intersectionEnd 
 	 */
	public PenetrationSweep(Vector2f normal, Vector2f sweepDir, Vector2f intersectionStart, Vector2f intersectionEnd) {
		super();
		this.normal = normal;
		this.sweepDir = sweepDir;
		this.startDist = intersectionStart.dot(sweepDir);
		this.endDist = intersectionEnd.dot(sweepDir);
	}


	/**
	 * 
	 * @param in 
	 * @param out 
	 * @param normal 
	 * @param vertsA 
	 * @param vertsB 
	 * @return 
	 */
	public static float getPenetrationDepth(Intersection in, Intersection out, Vector2f normal, Vector2f[] vertsA, Vector2f[] vertsB) {
		Vector2f sweepdir = new Vector2f(out.position);
		sweepdir.sub(in.position);
		
		PenetrationSweep ps = new PenetrationSweep(normal, sweepdir, in.position, out.position);
		

		ContourWalker walkerA = ps.new ContourWalker(vertsA, in.edgeA, out.edgeA, false);
		ContourWalker walkerB = ps.new ContourWalker(vertsB, (out.edgeB+1) % vertsB.length, (in.edgeB+1) % vertsB.length, true);

		float penetration = 0;
		float lowerBound = in.position.dot(normal);
		float upperBound = lowerBound;
			
		while ( walkerA.hasNext() || walkerB.hasNext() ) {
			if ( walkerA.hasNext() && 
					(walkerA.getNextDistance() < walkerB.getNextDistance() ||
							!walkerB.hasNext() ) ) {
				walkerA.next();
				if ( walkerA.getDistance() < ps.startDist || walkerA.getDistance() > ps.endDist )
					continue; 
				
				upperBound = walkerA.getPenetration();
				lowerBound = walkerB.getPenetration(walkerA.getDistance());
			} else {
				walkerB.next();
				if ( walkerB.getDistance() < ps.startDist || walkerB.getDistance() > ps.endDist )
					continue;
				
				upperBound = walkerA.getPenetration(walkerB.getDistance());
				lowerBound = walkerB.getPenetration();
			}
			
			penetration = Math.max(penetration, upperBound - lowerBound);
		}

		return penetration;
	}


	
	
	/**
	 */
	public class ContourWalker {
		
		/**  */
		private Vector2f[] verts;
		/**  */
		private int currentVert;
		/**  */
		private int firstVert;
		/**  */
		private int lastVert;
		/**  */
		private boolean isBackwards;
		
		/**  */
		private float distance;
		/**  */
		private float nextDistance;
		/**  */
		private float penetration;
		/**  */
		private float penetrationDelta;
		
		/**
		 * 
		 * @param verts  
		 * @param firstVert 
		 * @param lastVert 
		 * @param isBackwards 
		 */
		public ContourWalker(Vector2f[] verts, int firstVert, int lastVert, boolean isBackwards) {
			if ( firstVert < 0 || lastVert < 0 )
				throw new IllegalArgumentException("Vertex numbers cannot be negative.");
			
			if ( firstVert > verts.length || lastVert > verts.length )
				throw new IllegalArgumentException("The given vertex array doesn't include the first or the last vertex.");
			
			this.isBackwards = isBackwards;
			this.verts = verts;
			this.firstVert = firstVert;
			this.lastVert = lastVert;
			this.currentVert = isBackwards ? lastVert : firstVert;
			
			this.distance = verts[currentVert].dot(sweepDir);
			this.penetration = verts[currentVert].dot(normal);
			calculateNextValues();
		}
		
		/**
		 * @return 
		 */
		public float getDistance() {
			return distance;
		}
		
		/**
		 * 
		 * @return 
		 */
		public float getNextDistance() {
			if ( distance < startDist )
				return Math.min(nextDistance, startDist);
			if ( distance < endDist )
				return Math.min(nextDistance, endDist);
			
			return nextDistance;
		}
		
		/**
		 * @return 
		 */
		public float getPenetration() {
			return penetration;
		}
		
		/**
		 * 
		 * @param 
		 * @return 
		 */
		public float getPenetration(float distance) {
			return penetration + penetrationDelta * (distance - this.distance);
		}
		
		/**
		 */
		public void next() {
			if ( !hasNext() )
				return;
			
			if ( distance < startDist && nextDistance > startDist ) {
				this.penetration = getPenetration(startDist);
				this.distance = startDist;
				return;
			}
			
			if  ( distance < endDist && nextDistance > endDist ) {
				this.penetration = getPenetration(endDist);
				this.distance = endDist;
				return;
			}
			
			if ( isBackwards ) {
				currentVert = (currentVert - 1 + verts.length) % verts.length;
			} else {
				currentVert = (currentVert + 1) % verts.length;
			}
			
			distance = verts[currentVert].dot(sweepDir);
			penetration = verts[currentVert].dot(normal);
			calculateNextValues();
		}
		
		/**
		 */
		private void calculateNextValues() {
			int nextVert = isBackwards ? currentVert - 1 : currentVert + 1;
			nextVert = (nextVert + verts.length) % verts.length;
			
			nextDistance = verts[nextVert].dot(sweepDir);
			
			penetrationDelta = verts[nextVert].dot(normal) - penetration;
			if ( nextDistance == distance ) {
				penetration += penetrationDelta;
				penetrationDelta = 0;
			} else {
				penetrationDelta /= nextDistance - distance;
			}
		}
		
		/**
		 * @return 
		 */
		public boolean hasNext() {
			if ( distance < startDist && nextDistance > startDist )
				return true;
			
			if  ( distance < endDist && nextDistance > endDist )
				return true;
			
			int x = isBackwards ? lastVert - currentVert : currentVert - firstVert;
			x = (x + verts.length) % verts.length;
				
			x = (lastVert - firstVert + verts.length) % verts.length - x;
			
			return x > 0; 
		}
		
		/**
		 */
		public void reverse() {
			isBackwards = !isBackwards;
			
			calculateNextValues();
		}
	}

}
